/*
 * VisionProcess.cpp
 *
 *  Created on: Apr 19, 2010
 *      Author: Soheil Damangir
 */
#include "image.h"
#include "log.h"
#include "Detector.h"
#include "StripeDetector.h"
#include "Image2DDD.h"
#include "VisionProcess.h"
#include "../Util/Conf.h"
#include "time_tools.h"

using kthrobot::Log;
namespace Group5 {

VisionProcess::VisionProcess()
{
  Log::Write("----------------Initializing VisionProcess-----------------");
  {
    ifstream ifs(Conf::get().read<std::string>("BallTrainFile").c_str());
    if (!ifs.is_open())
    {
      Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    ball_detector = new Detector(ifs);
    Log::Write("Loading Ball Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("BombTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    bomb_detector = new Detector(ifs);
    Log::Write("Loading Bomb Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("StripeTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    stripe_detector = new StripeDetector(ifs);
    Log::Write("Loading Stripe Detector");
    ifs.close();
  }

  Image2DDD::t11 =  Conf::get().read<int>("T_11");
  Image2DDD::t12 =  Conf::get().read<int>("T_12");
  Image2DDD::t21 =  Conf::get().read<int>("T_21");
  Image2DDD::t22 =  Conf::get().read<int>("T_22");

  Image2DDD::bx =  Conf::get().read<int>("B_X");
  Image2DDD::by =  Conf::get().read<int>("B_Y");

  Log::Write("-----------------------------------------------------------");
}

void VisionProcess::ProcessImage(kthrobot::Image*image)
{
  last_image = image;
  normalize_image(last_image);
  ball_detector->detect(*last_image, ball_result,"");
  {
    int x=0,y=0;
    Image2DDD::Map(ball_result.x,ball_result.y, x, y);
    ball_result.x = x;
    ball_result.y = y;
  }
  //if (ball_result.size > 1452 && !stripe_reported) {
  //  stripe_detector->detect(*last_image, stripe_result, "");
  //  stripe_reported = true;
  //}
  //if (!ball_result.detected) stripe_reported = false;

  //bomb_detector->detect(*last_image, bomb_result, "");
  //TimeTools::WaitMS(500);
}

DetectionResults const & VisionProcess::getBallResult()
{
  return ball_result;
}
DetectionResults const & VisionProcess::getBombResult()
{
  return bomb_result;
}
DetectionResults const & VisionProcess::getStripeResult()
{
  return stripe_result;
}

VisionProcess::~VisionProcess()
{
  delete ball_detector;
  delete bomb_detector;
  delete stripe_detector;
  Log::Write("Deinitializing VisionProcess");
}
}
